/*
 * Chris Bena
 * 4/1/11
 * 
 * Converted old wall follow code to the new
 *  version of player with hokuyo_aist instead
 *  of laser proxy.
 *
 * KNOWN ISSUES:
 *  file output - opening the file but not writing
 *  occasional script version error - run again
 *  
 */
#include <libplayerc++/playerc++.h>
#include <hokuyo_aist/hokuyo_aist.h>
#include <hokuyo_aist/hokuyo_errors.h>
#include <flexiport/flexiport.h>

#include <iostream>
#include <fstream>

#include "args.h"

// Steps
#define FRONT 340
#define RIGHT 85
#define LEFT 595
#define FRONT_RIGHT 212
#define NEXT_RIGHT 88
#define FRONT_LEFT 405

// All ranges are in millimeters
#define MIN_RANGE 500  // min distance robot can be from the right wall
#define MAX_RANGE 1000 // max distance robot can be from the right wall
#define COLLISION 350  // the min range for collision detection
#define LOWERLIMIT 80  // lower limit to ignore garbage laser readings
#define DOOR_THRESHOLD 3 // depth of indent in wall made by closed doors

// Global stuff - robot, laser, position proxy
using namespace PlayerCc;

PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
hokuyo_aist::ScanData data;
hokuyo_aist::Sensor laser;

// State variable to tell if the current right wall is actually a door
bool door = false;
std::ofstream fout2;

void saveData2(double x, double y, double yaw, bool door){	
  fout2 << x; 
  fout2 << " ";;
  fout2 << y;
  fout2 << " ";
  fout2 << yaw;
  fout2 << " ";
  fout2 << data[RIGHT];
  fout2 << " ";
  fout2 << data[LEFT] << " ";
  fout2 << door << "\n";
}


/**
 * Checks for a door
 * use a hybrid of method 1 and method 2
 *    get ranges on the right and front right
 *    check the right range - when it has increased, now we've got something
 *    if that's true, then check front right range
 *       if frontRight is limited by a length of the door then return true
 *       else it's probably a hall - return false
 *
 **/
bool checkForDoor(double right, double nextRight){
  //laser.get_ranges(data);
  // now you can compare right and data
  std::cout << "frontRight: " << right << std::endl;
  std::cout << "nextRight: " << nextRight << std::endl;
  double diff = (nextRight - right);

  // Seen a door
  if(door == false && diff >= DOOR_THRESHOLD)
    return true;

  // No door
  else if(door == false && diff < DOOR_THRESHOLD)
    return false;

    // At a door and we finished the door
  else if(door == true && diff >= -DOOR_THRESHOLD)
    return false;
  
  //else (door == ture && diff == 0)
  // we are still next to the door
  else
    return true;

  /*
  if(door == false){
    // 
    //if( max of right range is greater than right ){
      //return true;
    //}
    //return false;

    if( (data[RIGHT] - right) > 100 ){
      if(frontRight > 0.0){
	return true;
      }
    }
    return false;
  }
  else{
    // in here, door is true and we need to check if we've past it
    // I think best way is to use frontRight but using right is easier
    // just check a range on the right and if it's all equal then door = false
    // if there's a chunk that's greater than the rest, than we're still next to the door
    if( (right - data[RIGHT]) > 0 ){
      return false
    }
    return true; // there is still a door
  }
  */
}

/**
 * Checks for a door
 * Uses a mask to find the door
 * First, check right
 *  if it's maxed out -> either open door or hallway
 *    if really is open door, then the range of maxed outedness will be limited by a certain length.
 *    if really hallway, the entire right side will be almost maxed out
 *
 *  if it's not maxed out -> check for closed door
 *   use other checkForDoor function
 **/
bool checkForDoor(){
  if(data[RIGHT] < 20){ // if right is maxed out
    return false;
  }

  else{
    return checkForDoor(data[RIGHT], data[NEXT_RIGHT]);
  }
}


void turn(double newspeed, double newturnrate){
  pp.SetSpeed(newspeed, newturnrate);
}

/**
 * Turns left - turns until lp[FRONT] is maxed out;
 * Does zero-point turn
 **/
void turn(){
  do{
    //pp.SetSpeed(0.1, -0.75);
    pp.SetSpeed(0.0, 0.375);
    laser.get_ranges(data);
    std::cout << "Still turning... " << data[FRONT] << "\n"; 
  }
  while(data[FRONT] < 5580 && data[FRONT]>=LOWERLIMIT);
}

/**
 * MAIN
 **/
int main(int argc, char **argv)
{
  parse_args(argc,argv);
  
  // we throw exceptions on creation if we fail
  try
    {
      pp.SetMotorEnable (true);
      
      double newspeed = 0;
      double newturnrate = 0;
      
      laser.open("type=serial,device=/dev/ttyACM0,timeout=2");
      laser.set_power(true);

      /* Open file out */

      fout2.open("7_0_DetectDoors.txt");
      if(!fout2.is_open()){
	std::cerr << "Error opening file\n";
	exit(-1);
      }

      fout2.setf(std::ios::fixed, std::ios::floatfield);
      fout2.precision(8);

      /* File open complete */
      
      for(;;){
	
	// this blocks until new data comes; 10Hz by default
	robot.Read(); 

	laser.get_ranges(data);
	// Using only the certain sensors - front, left, right
	double front = data[FRONT];
	double frontRight = data[FRONT_RIGHT];
	double nextRight = data[NEXT_RIGHT];
	double frontLeft = data[FRONT_LEFT];
	double right = data[RIGHT];
	double left = data[LEFT];	

	double yaw = pp.GetYaw();
	double minRight = 0;
	double minDistRight = 5600;
	double minDistLeft = 5600;
	
	
	for(int i = RIGHT; i <= FRONT; i++){
	  if(data[i] < minDistRight && data[i]>=LOWERLIMIT){
	    minDistRight=data[i];
	    minRight=i;
	  }
	}
	for(int i = FRONT; i <= LEFT; i++){
	  if(data[i] < minDistLeft && data[i]>=LOWERLIMIT)
	    minDistLeft=data[i];
	}

	// Check if door on the right
	door = checkForDoor(right, nextRight);

	// if inside range
	if(frontRight <= MIN_RANGE || minDistRight <= COLLISION){
	  std::cout << "inside of range  " << frontRight << " " << minDistRight << "\n";
	  newspeed = 0.0;
	  newturnrate = 0.175;
	  turn(newspeed, newturnrate);
	}
	
	// else if outside range
	if(frontRight > MAX_RANGE || minDistLeft <= COLLISION){
	  std::cout << "Outside range" << frontRight << " " << minDistLeft << "\n";
	  //std::cout << frontRight << "\n";
	  newspeed = 0.25;
	  newturnrate = -0.190;
	  turn(newspeed, newturnrate);
	}
	
	// else drive straight
	else{
	  std::cout << "Straight ahead\n";
	  newspeed = 0.5;
	  newturnrate = 0.0;
	  turn(newspeed, newturnrate);
	}
	
	// if reached a corner
	if(front >= LOWERLIMIT && frontRight >= LOWERLIMIT){
	  if(front <= 1500 && frontRight <= 800){
	    std::cout << "Turning a corner " << "\n";
	    turn();
	  }
	}
	
	if(door) std::cout << "DOOR "<< std::endl;

	saveData2(pp.GetXPos(), pp.GetYPos(), 0, door);
      }
      
    }
  
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }

  catch(hokuyo_aist::BaseError e)
    {
      std::cerr << e.what() << std::endl;
      return -1;
    }
}
  
